#include <iostream>
#include <fstream>
#include <string>
#include <cmath>
#include <limits>
#include <yaml-cpp/yaml.h>
#include <rclcpp/rclcpp.hpp>
#ifdef HAVE_NEW_YAMLCPP
namespace YAML {

// The >> operator disappeared in yaml-cpp 0.5, so this function is
// added to provide support for code written under the yaml-cpp 0.3 API.
template<typename T>
void operator >> (const YAML::Node& node, T& i) {
  i = node.as<T>();
}
} /* YAML */
#endif // HAVE_NEW_YAMLCPP

#include "calibration.h"

namespace lidar209_pointcloud 
{

const std::string NUM_LASERS = "num_lasers";
const std::string LASERS = "lasers";
const std::string LASER_ID = "laser_id";
const std::string x1 = "x1";
const std::string x2 = "x2";
const std::string x3 = "x3";
const std::string fix_offset = "fix_offset";
const std::string chs_offset = "chs_offset";
const std::string k1 = "k1";
const std::string k2 = "k2";
const std::string b = "b";
const std::string t1 = "T1";
const std::string t2 = "T2";
const std::string t3 = "T3";
const std::string t4 = "T4";
const std::string t5 = "T5";

const std::string ALPHA_CORR0 = "Alpha_corr0";
const std::string BETA_CORR0 = "Beta_corr0";
const std::string ROTATION = "Rotation_corr0";

const std::string ALPHA_CORR11 = "Alpha_corr11";
const std::string ALPHA_CORR12 = "Alpha_corr12";
const std::string ALPHA_CORR13 = "Alpha_corr13";
const std::string ALPHA_CORR14 = "Alpha_corr14";
const std::string ALPHA_CORR21 = "Alpha_corr21";
const std::string ALPHA_CORR22 = "Alpha_corr22";
const std::string ALPHA_CORR23 = "Alpha_corr23";
const std::string ALPHA_CORR24 = "Alpha_corr24";

const std::string Serial = "Serial";
const std::string Detector1 = "Detector1";
const std::string Detector2 = "Detector2";
const std::string Optics1 = "Optics1";
const std::string Optics2 = "Optics2";
const std::string Electro = "Electro";


const std::string DISTANCE_RESOLUTION = "distance_resolution";
/** Read calibration for a single laser. */
void operator >> (const YAML::Node& node,
                  std::pair<int, LaserCorrection>& correction)
{
  node[LASER_ID] >> correction.first;
  node[x1] >> correction.second.x1;
  node[x2] >> correction.second.x2;
  node[x3] >> correction.second.x3;
  node[k1] >> correction.second.k1;
  node[k2] >> correction.second.k2;
  node[b] >> correction.second.b;
  node[fix_offset] >> correction.second.fix_offset;
  node[chs_offset] >> correction.second.chs_offset;
  node[t1] >> correction.second.t1;
  node[t2] >> correction.second.t2;
  node[t3] >> correction.second.t3;
  node[t4] >> correction.second.t4;
  node[t5] >> correction.second.t5;
}

/** Read entire calibration file. */
void operator >> (const YAML::Node& node, Calibration& calibration)
{
  int num_lasers;
  node[NUM_LASERS] >> num_lasers;
  calibration.num_lasers = num_lasers;

  int alpha1;
  node[ALPHA_CORR11] >> alpha1;
  calibration.Alpha_corr11 = alpha1;

  int alpha2;
  node[ALPHA_CORR12] >> alpha2;
  calibration.Alpha_corr12 = alpha2;

  int alpha3;
  node[ALPHA_CORR13] >> alpha3;
  calibration.Alpha_corr13 = alpha3;

  int alpha4;
  node[ALPHA_CORR14] >> alpha4;
  calibration.Alpha_corr14 = alpha4;


  node[ALPHA_CORR21] >> alpha1;
  calibration.Alpha_corr21 = alpha1;


  node[ALPHA_CORR22] >> alpha2;
  calibration.Alpha_corr22 = alpha2;


  node[ALPHA_CORR23] >> alpha3;
  calibration.Alpha_corr23 = alpha3;

  node[ALPHA_CORR24] >> alpha4;
  calibration.Alpha_corr24 = alpha4;


  int alpha0;
  node[ALPHA_CORR0] >> alpha0;
  calibration.Alpha_corr0 = alpha0;

  node[BETA_CORR0] >> alpha0;
  calibration.Beta_corr0 = alpha0;

  node[ROTATION] >> alpha0;
  calibration.Rotation_corr0 = alpha0;

  int temp_num;
  node[Serial] >> temp_num;
  calibration.Serial = temp_num;

  node[Detector1] >> temp_num;
  calibration.Detector1 = temp_num;

  node[Detector2] >> temp_num;
  calibration.Detector2 = temp_num;

  node[Optics1] >> temp_num;
  calibration.Optics1 = temp_num;

  node[Optics2] >> temp_num;
  calibration.Optics2 = temp_num;

  node[Electro] >> temp_num;
  calibration.Electro = temp_num;

  float distance_resolution_m;
  node[DISTANCE_RESOLUTION] >> distance_resolution_m;
  calibration.distance_resolution_m = distance_resolution_m;
  const YAML::Node& lasers = node[LASERS];
  calibration.laser_corrections.clear();
  calibration.laser_corrections.resize(num_lasers);
  for (int i = 0; i < num_lasers; i++) {
    std::pair<int, LaserCorrection> correction;
    lasers[i] >> correction;
    const int index = correction.first;
    if( index >= calibration.laser_corrections.size() )
    {
      calibration.laser_corrections.resize( index+1 );
    }
    calibration.laser_corrections[index] = (correction.second);
    //   calibration.laser_corrections_map.insert(correction);
  }

  //  for (int i=0;i<16;++i)
  //  {
  //    calibration.laser_corrections[i].A_corr=alpha1;
  //    calibration.laser_corrections[i+16].A_corr=alpha2;
  //    calibration.laser_corrections[i+32].A_corr=alpha3;
  //    calibration.laser_corrections[i+48].A_corr=alpha4;
  //  }

}


//YAML::Emitter& operator << (YAML::Emitter& out,
//                            const std::pair<int, LaserCorrection> correction)
//{
//  out << YAML::BeginMap;
//  out << YAML::Key << LASER_ID << YAML::Value << correction.first;
//  out << YAML::Key << ROT_CORRECTION <<
//         YAML::Value << correction.second.rot_correction;
//  out << YAML::Key << VERT_CORRECTION <<
//         YAML::Value << correction.second.vert_correction;
//  out << YAML::Key << DIST_CORRECTION <<
//         YAML::Value << correction.second.dist_correction;
//  out << YAML::Key << TWO_PT_CORRECTION_AVAILABLE <<
//         YAML::Value << correction.second.two_pt_correction_available;
//  out << YAML::Key << DIST_CORRECTION_X <<
//         YAML::Value << correction.second.dist_correction_x;
//  out << YAML::Key << DIST_CORRECTION_Y <<
//         YAML::Value << correction.second.dist_correction_y;
//  out << YAML::Key << VERT_OFFSET_CORRECTION <<
//         YAML::Value << correction.second.vert_offset_correction;
//  out << YAML::Key << HORIZ_OFFSET_CORRECTION <<
//         YAML::Value << correction.second.horiz_offset_correction;
//  out << YAML::Key << MAX_INTENSITY <<
//         YAML::Value << correction.second.max_intensity;
//  out << YAML::Key << MIN_INTENSITY <<
//         YAML::Value << correction.second.min_intensity;
//  out << YAML::Key << FOCAL_DISTANCE <<
//         YAML::Value << correction.second.focal_distance;
//  out << YAML::Key << FOCAL_SLOPE <<
//         YAML::Value << correction.second.focal_slope;
//  out << YAML::EndMap;
//  return out;
//}

//YAML::Emitter& operator <<
//(YAML::Emitter& out, const Calibration& calibration)
//{
//  out << YAML::BeginMap;
//  out << YAML::Key << NUM_LASERS <<
//         YAML::Value << calibration.laser_corrections.size();
//  out << YAML::Key << LASERS << YAML::Value << YAML::BeginSeq;
//  for (std::map<int, LaserCorrection>::const_iterator
//       it = calibration.laser_corrections.begin();
//       it != calibration.laser_corrections.end(); it++)
//  {
//    out << *it;
//  }
//  out << YAML::EndSeq;
//  out << YAML::EndMap;
//  return out;
//}

void Calibration::read(const std::string& calibration_file) {
  std::ifstream fin(calibration_file.c_str());
  if (!fin.is_open()) {
    //ROS_INFO("here0");

    initialized = false;
    return;
  }

  initialized = true;
  try {
    YAML::Node doc;
#ifdef HAVE_NEW_YAMLCPP
  //  ROS_INFO("here1");
    fin.close();
    doc = YAML::LoadFile(calibration_file);
#else
    YAML::Parser parser(fin);
    parser.GetNextDocument(doc);
#endif
    //ROS_INFO("here2");
    doc >> *this;
  } catch (YAML::Exception &e) {
    std::cerr << "YAML Exception: " << e.what() << std::endl;
    initialized = false;
  }
  fin.close();
}

//void Calibration::write(const std::string& calibration_file) {
//  std::ofstream fout(calibration_file.c_str());
//  YAML::Emitter out;
//  out << *this;
//  fout << out.c_str();
//  fout.close();
//}

} /* lidar209_pointcloud */
